/**
 * @file mpu6050.c
 * @author chushang (email:chushangcs@163.com)
 * @brief 解算MPU6050数据
 * @version 0.1
 * @date 2025-04-28
 *
 * @copyright Copyright (c) 2025
 *
 */

// #include "stm32f1xx_hal.h"
#include "mpu6050.h"

#include "math.h"
#include "mpu6050_reg.h"

#define COMPENSATE_FACTOR 0.95238 // 互补滤波系数

static MPU6050_RawData rd = {0};
// IMU_Data               imu_data = {0};

/**
 * @brief 主机控制SCL
 *
 * @param val [in] SCL的状态,0或1
 */
static void IIC_WriteSCL(uint8_t val)
{
    HAL_GPIO_WritePin(IIC_SCL_GPIO_PORT, IIC_SCL_GPIO_PIN, (GPIO_PinState)val);
    // DWT_Delay_us(10);
}

/**
 * @brief 主机控制SDA
 *
 * @param val [in] SDA的状态,0或1
 */
static void IIC_WriteSDA(uint8_t val)
{
    HAL_GPIO_WritePin(IIC_SDA_GPIO_PORT, IIC_SDA_GPIO_PIN, (GPIO_PinState)val);
    // DWT_Delay_us(10);
}

/**
 * @brief 读取SDA的状态
 *
 * @return SDA线的状态,0或1
 */
static GPIO_PinState IIC_ReadSDA(void)
{
    uint8_t ret;
    ret = HAL_GPIO_ReadPin(IIC_SDA_GPIO_PORT, IIC_SDA_GPIO_PIN);
    // DWT_Delay_us(10);
    return ret;
}

/**
 * @brief IIC开始时序,SCL高电平期间拉低SDA
 *
 */
static void IIC_Start(void)
{
    IIC_WriteSDA(1);
    IIC_WriteSCL(1);
    IIC_WriteSDA(0);
    IIC_WriteSCL(0); // 拉低SCL,方便拼接下一个时序
}

/**
 * @brief IIC结束时序,SCL高电平期间释放SDA
 *
 */
static void IIC_Stop(void)
{
    IIC_WriteSDA(0);
    IIC_WriteSCL(1);
    IIC_WriteSDA(1);
}

/**
 * @brief 将字节发送到IIC总线上
 *
 * @param byte [in] 待发送的字节
 */
static void IIC_SendByte(uint8_t byte)
{
    int i;
    for (i = 0; i < 8; i++)
    {
        IIC_WriteSDA(byte & (0x80 >> i)); // 高位先行
        IIC_WriteSCL(1);                  // 释放SCL,从机会在SCL高电平期间读取SDA的数据
        // DWT_Delay_us(5);                  // 延长SCL高电平时间
        IIC_WriteSCL(0); // 拉低SCL,主机开始发送下一位数据
    }
}

/**
 * @brief 接收IIC总线上的字节数据
 *
 * @return 接收到的字节数据
 */
static uint8_t IIC_ReceiveByte(void)
{
    uint8_t ret = 0x00;
    IIC_WriteSDA(1); // 主机先释放SDA,避免干扰从机的数据
    for (int i = 0; i < 8; ++i)
    {
        IIC_WriteSCL(1); // 主机释放SCL,此时从机不允许操纵SDA
        // DWT_Delay_us(5);
        if (IIC_ReadSDA() != 0) // 主机读取SDA的数据
        {
            ret |= (0x80 >> i);
        }
        IIC_WriteSCL(0); // 主机拉低SCL,从机将下一位数据放到SDA
    }
    return ret;
}

/**
 * @brief 发送应答位
 *
 * @param ack [in] 应答位,0或1
 */
static void IIC_SendACK(uint8_t ack)
{
    IIC_WriteSDA(ack); // 主机发送应答位
    IIC_WriteSCL(1);   // 主机释放SCL,从机将在此时读取SDA的数据
    // DWT_Delay_us(5);
    IIC_WriteSCL(0); // 拉低SCL,方便拼接下一个时序
}

/**
 * @brief 接收总线上的应答位
 *
 * @return 应答位,0应答,1非应答
 */
static uint8_t IIC_ReceiveACK(void)
{
    uint8_t ack = 1;     // 默认无应答
    IIC_WriteSDA(1);     // 主机释放SDA,避免干扰到从机的SDA数据
    IIC_WriteSCL(1);     // 释放SCL,此时从机不允许发送SDA
    ack = IIC_ReadSDA(); // 读取从机发送的应答位
    IIC_WriteSCL(0);     // 拉低SCL,方便拼接下一个时序
    return ack;
}

/**
 * @brief 往MPU6050的寄存器写入数据
 *
 * @param reg [in] 寄存器地址
 * @param val [in] 要写入的数据
 */
void MPU6050_WriteReg(uint8_t reg, uint8_t val)
{
    IIC_Start();

    IIC_SendByte(MPU6050_ADDR | IIC_W_MASK);
    if (IIC_ReceiveACK())
    {
        // Error_Set(__FUNCTION__, __LINE__, __FILE__);
    }

    IIC_SendByte(reg);
    if (IIC_ReceiveACK())
    {
        // Error_Set(__FUNCTION__, __LINE__, __FILE__);
    }

    IIC_SendByte(val);
    if (IIC_ReceiveACK())
    {
        // Error_Set(__FUNCTION__, __LINE__, __FILE__);
    }

    IIC_Stop();
}

/**
 * @brief 读取MPU6050寄存器的值
 *
 * @param reg [in] 读取的寄存器地址
 * @return 该寄存器的值
 */
uint8_t MPU6050_ReadReg(uint8_t reg)
{
    uint8_t data = 0;

    IIC_Start();

    IIC_SendByte(MPU6050_ADDR | IIC_W_MASK);
    if (IIC_ReceiveACK()) // 1是非应答
    {
        // Error_Set(__FUNCTION__, __LINE__, __FILE__);
    }

    IIC_SendByte(reg);
    if (IIC_ReceiveACK())
    {
        // Error_Set(__FUNCTION__, __LINE__, __FILE__);
    }

    IIC_Start();

    IIC_SendByte(MPU6050_ADDR | IIC_R_MASK);
    if (IIC_ReceiveACK())
    {
        // Error_Set(__FUNCTION__, __LINE__, __FILE__);
    }

    data = IIC_ReceiveByte();
    IIC_SendACK(1); // 主机给1,非应答

    IIC_Stop();

    return data;
}

/**
 * @brief 初始化MPU6050
 *
 */
void MPU6050_Init(void)
{
    /*MPU6050寄存器初始化，需要对照MPU6050手册的寄存器描述配置，此处仅配置了部分重要的寄存器*/
    MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x01);   // 电源管理寄存器1，取消睡眠模式，选择时钟源为X轴陀螺仪
    MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00);   // 电源管理寄存器2，保持默认值0，所有轴均不待机
    MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09);   // 采样率分频寄存器，配置采样率
    MPU6050_WriteReg(MPU6050_CONFIG, 0x06);       // 配置寄存器，配置DLPF
    MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18);  // 陀螺仪配置寄存器，选择满量程为±2000°/s
    MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x18); // 加速度计配置寄存器，选择满量程为±16g
}

/**
 * @brief 获取MPU6050的ID号
 *
 * @return MPU6050的ID号: 0x68
 */
uint8_t MPU6050_GetID(void) { return MPU6050_ReadReg(MPU6050_WHO_AM_I); }

/**
 * @brief 获取MPU6050的三轴加速度和角加速度
 *
 * @param rd [out] 存放数据的指针
 */
void MPU6050_GetRawData(MPU6050_RawData *rd)
{
    uint8_t dh, dl;
    dh         = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H); // 读取加速度计X轴的高8位数据
    dl         = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L); // 读取加速度计X轴的低8位数据
    rd->acc[X] = (dh << 8) | dl;                        // 数据拼接，通过输出参数返回

    dh         = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_H); // 读取加速度计Y轴的高8位数据
    dl         = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_L); // 读取加速度计Y轴的低8位数据
    rd->acc[Y] = (dh << 8) | dl;                        // 数据拼接，通过输出参数返回

    dh         = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_H); // 读取加速度计Z轴的高8位数据
    dl         = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_L); // 读取加速度计Z轴的低8位数据
    rd->acc[Z] = (dh << 8) | dl;                        // 数据拼接，通过输出参数返回

    dh          = MPU6050_ReadReg(MPU6050_GYRO_XOUT_H); // 读取陀螺仪X轴的高8位数据
    dl          = MPU6050_ReadReg(MPU6050_GYRO_XOUT_L); // 读取陀螺仪X轴的低8位数据
    rd->gyro[X] = (dh << 8) | dl;                       // 数据拼接，通过输出参数返回

    dh          = MPU6050_ReadReg(MPU6050_GYRO_YOUT_H); // 读取陀螺仪Y轴的高8位数据
    dl          = MPU6050_ReadReg(MPU6050_GYRO_YOUT_L); // 读取陀螺仪Y轴的低8位数据
    rd->gyro[Y] = (dh << 8) | dl;                       // 数据拼接，通过输出参数返回

    dh          = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H); // 读取陀螺仪Z轴的高8位数据
    dl          = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L); // 读取陀螺仪Z轴的低8位数据
    rd->gyro[Z] = (dh << 8) | dl;                       // 数据拼接，通过输出参数返回

    dh       = MPU6050_ReadReg(MPU6050_TEMP_OUT_H); // 读取温度计的高8位数据
    dl       = MPU6050_ReadReg(MPU6050_TEMP_OUT_L); // 读取温度计的低8位数据
    rd->temp = (dh << 8) | dl;
}

/**
 * @brief 互补滤波解算欧拉角
 *
 * @param id     [out] 指向存放解算后的欧拉角的指针
 * @param period [in]  解算周期,单位s
 */
void MPU6050_GetIMUData(IMU_Data *id, float period)
{
    static float ax, ay, az, gx, gy, gz;
    static float acc_pitch, acc_roll;             // 通过加速度解算的欧拉角
    static float gyro_yaw, gyro_pitch, gyro_roll; // 通过陀螺仪解算的欧拉角

    MPU6050_GetRawData(&rd);

    // 因为选择的满量程为16g,所以将其缩放回真实值
    ax = rd.acc[X] * 16.0f / 32768;
    ay = rd.acc[Y] * 16.0f / 32768;
    az = rd.acc[Z] * 16.0f / 32768;

    // 因为选择的满量程为2000°/s,所以将其缩放回真实值
    gx = rd.gyro[X] * 2000.0f / 32768;
    gy = rd.gyro[Y] * 2000.0f / 32768;
    gz = rd.gyro[Z] * 2000.0f / 32768;

    id->temp = rd.temp / 340.0f + 36.53f;

    // 通过陀螺仪解算
    gyro_yaw += gz * period;
    gyro_pitch += gx * period;
    gyro_roll -= gy * period;

    // 通过加速度解算
    acc_pitch = atan2f(ax, az) * 180.0f / 3.14159f;
    acc_roll  = atan2f(ay, az) * 180.0f / 3.14159f;

    // 互补滤波
    id->yaw   = gyro_yaw;
    id->pitch = COMPENSATE_FACTOR * gyro_pitch + (1 - COMPENSATE_FACTOR) * acc_pitch;
    id->roll  = COMPENSATE_FACTOR * gyro_roll + (1 - COMPENSATE_FACTOR) * acc_roll;
}